"""
GomerX 导航模块
用于实现机器人的导航功能，包括循迹和目标导航
"""

import time
from modules.vision import VisionProcessor
from modules.chassis import Chassis

class Navigator:
    """GomerX 导航控制器"""
    
    def __init__(self, chassis, camera):
        """
        初始化导航器
        
        Args:
            chassis: 底盘控制对象
            camera: 摄像头对象
        """
        self.chassis = chassis
        self.camera = camera
        self.vision_processor = VisionProcessor()
        self.is_navigating = False
        
    def start_navigation(self):
        """开始导航"""
        if not self.chassis.is_connected:
            print("底盘未连接，无法开始导航")
            return False
            
        if not self.camera.is_opened:
            print("摄像头未打开，无法开始导航")
            return False
            
        self.is_navigating = True
        self.chassis.reset_pid()  # 重置PID控制器
        print("开始循迹导航")
        return True
    
    def stop_navigation(self):
        """停止导航"""
        self.is_navigating = False
        self.chassis.stop()
        print("停止导航")
    
    def follow_line(self, max_duration=None):
        """
        循迹导航
        
        Args:
            max_duration: 最大循迹时间(秒)，如果为None则无限制
            
        Returns:
            bool: 是否成功完成循迹
        """
        if not self.start_navigation():
            return False
            
        start_time = time.time()
        lost_line_count = 0
        max_lost_line = 10  # 连续丢失线条的最大次数
        
        try:
            while self.is_navigating:
                # 检查是否超时
                if max_duration and (time.time() - start_time) > max_duration:
                    print("循迹超时")
                    break
                
                # 获取摄像头图像
                frame = self.camera.read_frame()
                if frame is None:
                    print("无法获取摄像头图像")
                    time.sleep(0.1)
                    continue
                
                # 检测线条
                lines = self.vision_processor.detect_lines(frame)
                
                if lines:
                    # 计算线条偏移
                    offset = self.vision_processor.calculate_line_offset(lines, frame.shape[1])
                    
                    # 控制底盘循迹
                    self.chassis.follow_line(offset)
                    
                    # 重置丢失线条计数
                    lost_line_count = 0
                else:
                    # 丢失线条，尝试恢复
                    lost_line_count += 1
                    print(f"丢失线条 {lost_line_count}/{max_lost_line}")
                    
                    if lost_line_count >= max_lost_line:
                        print("连续丢失线条过多，停止循迹")
                        break
                    
                    # 尝试小幅转动寻找线条
                    self.chassis.turn_left(angle=0.1)
                    time.sleep(0.2)
                
                # 短暂延迟，避免过度占用CPU
                time.sleep(0.05)
                
            return True
            
        except Exception as e:
            print(f"循迹过程中发生错误: {e}")
            return False
        finally:
            self.stop_navigation()
    
    def navigate_to_marker(self, marker_type, max_duration=30):
        """
        导航到指定标记区域
        
        Args:
            marker_type: 标记类型，如 "item_zone", "drop_zone"
            max_duration: 最大导航时间(秒)
            
        Returns:
            bool: 是否成功到达标记区域
        """
        if not self.start_navigation():
            return False
            
        start_time = time.time()
        
        try:
            while self.is_navigating:
                # 检查是否超时
                if (time.time() - start_time) > max_duration:
                    print(f"导航到 {marker_type} 超时")
                    break
                
                # 获取摄像头图像
                frame = self.camera.read_frame()
                if frame is None:
                    print("无法获取摄像头图像")
                    time.sleep(0.1)
                    continue
                
                # 这里应该有识别特定标记的逻辑
                # 由于没有具体的标记定义，这里简化处理
                # 实际应用中应该根据标记的形状、颜色等特征进行识别
                
                # 简化处理：循迹导航一段时间后停止
                # 实际应用中应该检测到标记后才停止
                lines = self.vision_processor.detect_lines(frame)
                
                if lines:
                    # 计算线条偏移
                    offset = self.vision_processor.calculate_line_offset(lines, frame.shape[1])
                    
                    # 控制底盘循迹
                    self.chassis.follow_line(offset)
                else:
                    # 停止移动，认为已到达标记区域
                    self.chassis.stop()
                    print(f"已到达 {marker_type}")
                    return True
                
                # 短暂延迟，避免过度占用CPU
                time.sleep(0.05)
                
            return False
            
        except Exception as e:
            print(f"导航到 {marker_type} 过程中发生错误: {e}")
            return False
        finally:
            self.stop_navigation()
    
    def adjust_position_to_object(self, object_offset, max_attempts=5):
        """
        调整机器人位置，使物体位于合适位置
        
        Args:
            object_offset: 物体相对于图像中心的偏移量，范围[-1, 1]
            max_attempts: 最大调整次数
            
        Returns:
            bool: 是否成功调整到合适位置
        """
        if not self.chassis.is_connected:
            print("底盘未连接")
            return False
            
        # 如果物体已经在中心位置，不需要调整
        if abs(object_offset) < 0.1:
            return True
            
        attempts = 0
        
        while attempts < max_attempts:
            attempts += 1
            
            # 根据偏移量调整位置
            if object_offset > 0:  # 物体在右侧，需要右转
                self.chassis.turn_right(angle=0.1)
            else:  # 物体在左侧，需要左转
                self.chassis.turn_left(angle=0.1)
            
            time.sleep(0.5)
            
            # 重新获取图像并计算偏移
            frame = self.camera.read_frame()
            if frame is None:
                continue
                
            # 这里应该重新检测物体并计算偏移
            # 由于没有传入物体检测参数，这里简化处理
            # 实际应用中应该重新检测物体位置
            
            # 简化处理：假设调整成功
            print(f"位置调整尝试 {attempts}/{max_attempts}")
            break
            
        return attempts < max_attempts